// agmatthews USERHOOKS
#include <JETI_EX_SENSOR.h>
#include <avr/pgmspace.h>

int32_t jetiHauteur = 0;
//uint8_t jetiDate[3] = {0,0,0};
//uint8_t jetiTime[3] = {0,0,0};
int32_t jetiVoltage = 0;
int32_t jetiCourant = 0;

/* GPS */
uint8_t jetiGpsLatitude[4] = {
  'N',0,0,0};
uint8_t jetiGpsLongitude[4] = {
  'E',0,0,0};
int32_t jetiAltitude = 0;
int32_t jetiNumSats = 0;
uint8_t jetiGpsLatitudeDataNb = 0;
uint8_t jetiGpsLongitudeDataNb = 0;
uint8_t jetiGpsAltitudeDataNb = 0;
GPS::GPS_Status jetiLastGpsStatus = GPS::NO_GPS;
uint16_t gpsMinute = 0;
int32_t jetiGpsQuality = 0;
int32_t jetiGpsSpeed = 0;

uint8_t jetiSelect = 0;

bool lastMotorArmed=false;
uint16_t beforeRepeatAlarm = 0; 

void userhook_init()
{
  // put your initialisation code here
  JETI_JetiBox_P((const prog_char*)PSTR(THISFIRMWARE),"");
  JETI_Init((const prog_char*)PSTR("ArduCopter"));
  JETI_setValue(JETI_addData((const prog_char*)PSTR("Rel. altit"),(const prog_char*)PSTR("m")),&jetiHauteur,1);   /* OK */
  JETI_setValue(JETI_addData((const prog_char*)PSTR("Voltage"),(const prog_char*)PSTR("V")),&jetiVoltage,1);
  JETI_setValue(JETI_addData((const prog_char*)PSTR("Current"),(const prog_char*)PSTR("A")),&jetiCourant,1);

  jetiGpsLatitudeDataNb = JETI_addData((const prog_char*)PSTR("Latitude"),(const prog_char*)PSTR(""));   /* OK */
  jetiGpsLongitudeDataNb = JETI_addData((const prog_char*)PSTR("Longitude"),(const prog_char*)PSTR(""));   /* OK */
  JETI_setValue(JETI_addData((const prog_char*)PSTR("Quality"),(const prog_char*)PSTR("")),&jetiGpsQuality);
  JETI_setValue(JETI_addData((const prog_char*)PSTR("SatCount"),(const prog_char*)PSTR("")),&jetiNumSats);
  jetiGpsAltitudeDataNb = JETI_addData((const prog_char*)PSTR("Altitude"),(const prog_char*)PSTR("m"));
  JETI_setValue(JETI_addData((const prog_char*)PSTR("Speed"),(const prog_char*)PSTR("m/s")),&jetiGpsSpeed);

}

void userhook_50Hz()
{
  // put your 50Hz code here

  if (!lastMotorArmed&&motors.armed()) {
    JETI_alarm('F');
    beforeRepeatAlarm=1;
  }
  if (beforeRepeatAlarm) {
    beforeRepeatAlarm--;
  } 
  else {
    if ((6<battery_voltage1)&&(battery_voltage1<(3.4*3))&&lastMotorArmed) {
      JETI_alarm('Z');
      beforeRepeatAlarm=250;
    }
  }
  lastMotorArmed=motors.armed();

  jetiSelect--;

  if (jetiSelect==0) {
    jetiSelect=3;

    jetiHauteur = (current_loc.alt - home.alt)/10;
    jetiVoltage = int32_t(battery_voltage1*10) ;
    jetiCourant = int32_t(current_amps1*10) ;

    if ((jetiLastGpsStatus != GPS::GPS_OK)&&(g_gps->status() == GPS::GPS_OK)) {
      JETI_setValueGPS(jetiGpsLatitudeDataNb,jetiGpsLatitude);
      JETI_setValueGPS(jetiGpsLongitudeDataNb,jetiGpsLongitude);
      JETI_setValue(jetiGpsAltitudeDataNb,&jetiAltitude);
      jetiGpsQuality=1;
    }
    if ((jetiLastGpsStatus == GPS::GPS_OK)&&(g_gps->status() != GPS::GPS_OK)) {
      JETI_unsetValue(jetiGpsLatitudeDataNb);
      JETI_unsetValue(jetiGpsLongitudeDataNb);
      JETI_unsetValue(jetiGpsAltitudeDataNb);
      jetiGpsQuality=0;
      jetiGpsSpeed=0;
    }
    jetiLastGpsStatus=g_gps->status();
    jetiGpsLatitude[1]=g_gps->latitude/10000000;
    gpsMinute = (g_gps->latitude%10000000)*6/1000 ;
    jetiGpsLatitude[2]=(gpsMinute&0xff00)>>8;
    jetiGpsLatitude[3]=gpsMinute&0x00ff;
    jetiGpsLongitude[1]=g_gps->longitude/10000000;
    gpsMinute = (g_gps->longitude%10000000)*6/1000 ;
    jetiGpsLongitude[2]=(gpsMinute&0xff00)>>8;
    jetiGpsLongitude[3]=gpsMinute&0x00ff;
    jetiAltitude = g_gps->altitude/100;
    jetiNumSats = g_gps->num_sats;
    jetiGpsSpeed=g_gps->ground_speed/100;


  }
  if (!JETI_createFrame()) {
    jetiSelect=1;
  }

}



